#ifndef __ASTAR_H__
#define __ASTAR_H__

#include <algorithm>  // 添加 reverse
#include <cmath>
#include <cstdlib>  // 添加 abs
#include <functional>
#include <map>
#include <queue>
#include <type_traits>  // 添加 type_traits
#include <vector>

// 二维点结构
struct Point2D
{
    double x, y;

    Point2D(double x = 0, double y = 0) : x(x), y(y) {}
    bool operator==(const Point2D& other) const { return x == other.x && y == other.y; }

    // 添加哈希支持
    bool operator<(const Point2D& other) const
    {
        if (x != other.x)
            return x < other.x;
        return y < other.y;
    }
};

// 三维点结构
struct Point3D
{
    double x, y, z;

    Point3D(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}
    bool operator==(const Point3D& other) const
    {
        return x == other.x && y == other.y && z == other.z;
    }

    // 添加哈希支持
    bool operator<(const Point3D& other) const
    {
        if (x != other.x)
            return x < other.x;
        if (y != other.y)
            return y < other.y;
        return z < other.z;
    }
};

// 通用的A*算法实现（支持2D/3D）
template <typename Point>
bool aStarSearch(const Point& start, const Point& goal,
                 std::function<bool(const Point&)> isObstacle, std::vector<Point>& path)
{
    // 节点结构
    struct AStarNode
    {
        Point position;
        double g;  // 实际代价
        double f;  // 估计总代价
        Point parent;

        AStarNode(Point p, double g, double f, Point parent = Point())
            : position(p), g(g), f(f), parent(parent)
        {
        }

        bool operator>(const AStarNode& other) const { return f > other.f; }
    };

    std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode>> openSet;
    std::map<Point, double> gScore;
    std::map<Point, Point> cameFrom;

    // 计算启发式距离（通用函数）
    auto heuristic = [](const Point& a, const Point& b) -> double
    {
        if constexpr (std::is_same_v<Point, Point2D>)
        {
            double dx = a.x - b.x;
            double dy = a.y - b.y;
            return std::sqrt(dx * dx + dy * dy);
        }
        else
        {
            double dx = a.x - b.x;
            double dy = a.y - b.y;
            double dz = a.z - b.z;
            return std::sqrt(dx * dx + dy * dy + dz * dz);
        }
    };

    // 生成邻居方向（支持2D/3D）
    std::vector<Point> directions;

    if constexpr (std::is_same_v<Point, Point2D>)
    {
        directions = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}, {1, 1}, {-1, 1}, {1, -1}, {-1, -1}};
    }
    else
    {
        directions = {{1, 0, 0},   {-1, 0, 0},  {0, 1, 0},  {0, -1, 0},  {0, 0, 1},  {0, 0, -1},
                      {1, 1, 0},   {1, -1, 0},  {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1},  {1, 0, -1},
                      {-1, 0, 1},  {-1, 0, -1}, {0, 1, 1},  {0, 1, -1},  {0, -1, 1}, {0, -1, -1},
                      {1, 1, 1},   {1, 1, -1},  {1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1},
                      {-1, -1, 1}, {-1, -1, -1}};
    }

    // 起点初始化
    openSet.push(AStarNode(start, 0, heuristic(start, goal)));
    gScore[start] = 0;
    cameFrom[start] = start;  // 修改：起点父节点设为自身，避免后续路径重建问题

    while (!openSet.empty())
    {
        AStarNode current = openSet.top();
        openSet.pop();

        // 跳过过期节点
        auto it = gScore.find(current.position);
        if (it != gScore.end() && current.g > it->second)
        {
            continue;
        }

        // 到达目标
        if (current.position == goal)
        {
            Point currentPos = goal;

            // 修改路径重建逻辑，防止无限循环
            while (!(currentPos == start))
            {
                path.push_back(currentPos);
                currentPos = cameFrom[currentPos];
            }
            path.push_back(start);
            std::reverse(path.begin(), path.end());
            return true;
        }

        // 探索邻居
        for (const Point& dir : directions)
        {
            Point neighbor;
            if constexpr (std::is_same_v<Point, Point2D>)
            {
                neighbor = Point2D(current.position.x + dir.x, current.position.y + dir.y);
            }
            else
            {
                neighbor = Point3D(current.position.x + dir.x, current.position.y + dir.y,
                                   current.position.z + dir.z);
            }

            // 跳过障碍物
            if (isObstacle(neighbor))
                continue;

            // 计算移动代价（对角线/三维对角线代价更高）
            double moveCost = 1.0;
            if constexpr (std::is_same_v<Point, Point2D>)
            {
                if (std::abs(dir.x) > 0 && std::abs(dir.y) > 0)
                {
                    moveCost = 1.414;
                }
            }
            else
            {
                int count = (std::abs(dir.x) > 0) + (std::abs(dir.y) > 0) + (std::abs(dir.z) > 0);
                if (count == 2)
                    moveCost = 1.414;
                else if (count == 3)
                    moveCost = 1.732;  // √3 ≈ 1.732
            }

            double tentativeG = current.g + moveCost;

            // 检查是否找到更好路径
            it = gScore.find(neighbor);
            if (it == gScore.end() || tentativeG < it->second)
            {
                gScore[neighbor] = tentativeG;
                cameFrom[neighbor] = current.position;

                double h = heuristic(neighbor, goal);
                openSet.push(AStarNode(neighbor, tentativeG, tentativeG + h));
            }
        }
    }

    return false;  // 未找到路径
}

#endif  //__ASTAR_H__